/**************************************************************************
   Tests the getPressure functions
 **************************************************************************/
#include <Adafruit_BMP280.h>
#include <PID_v1.h>
#include <SoftWire.h>
#include <QC3Control.h>
// #include <U8g2lib.h>
#define DEBUG

#define PWM_OUTPUT_PIN (PB3)
#define POTENTIOMETER_INPUT_PIN (PA0)
#define MODE_SELECT_INPUT_PIN (PA1)
#define SPEED_PULSE_INPUT (PB0)
#define BOOT0_PIN (PB6)
#define DC_CHECK (PB2)
#define QC_DATA_P (PA4)
#define QC_DATA_N (PB1)
#define VOLTAGE_DETECTION (PA5)
#define CURRENT_DETECTION (PA6)
#define LED_OUTPUT_PIN (PA7)

#define MANDATORY_USE_OF_QC_PROTOCOL (1)

SoftWire SWire;
Adafruit_BMP280 bmp_intel(&SWire);
Adafruit_BMP280 bmp_env(&SWire);
QC3Control quickCharge(QC_DATA_P, QC_DATA_N);  //D+ D-

#define PWM_MIN (40)
#define PWM_MAX (255)

//防止外部电位器死区
#define POTENTIOMETER_INPUT_MIN (32)
#define POTENTIOMETER_INPUT_MAX (4095 - 32)

#define PID_CAL_TIME (70)

#ifdef Serial
int putchar(int ch)
{
	Serial.write(ch);
	Serial.flush();
	return 1;
}
#else
	#undef DEBUG
#endif

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
double Kp = 40, Ki = 5, Kd = 1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, P_ON_M, DIRECT);

uint8_t pwm = 0;
uint8_t pwm_real = (PWM_MAX - PWM_MIN)/2;
uint8_t lastPwm = 0;
uint64_t pwm_last_adjust_timer = 0;
float instantaneous_power_last = 0;
uint8_t pwm_adjust_interval = 25;
float instantaneous_power_detla=0;
int potentiometerValue;

//速度脉冲 捕获 定时器
#define SPEED_RATIO (8)
HardwareTimer timer(TIM3);
uint32_t speedPulseTimer = 0;
uint32_t speedPulseCount = 0;
uint32_t speedPulse = 0;

#define PRESSURE_STABLE_DELTA (10)
float pressure_inter_last;
float pressure_inter = 1003;
float pressure_env = 1003;
float pressure_inter_max_delta;

#define BMP280_SAMPLING_TIME (70)
uint64_t bmp280_sampling_timer = 0;

//PID计算定时器
uint64_t mode_auto_timer = 0;

//系统电压
float sys_vol = 0;
//系统电流
float sys_cur = 0;

//led闪烁定时器
uint64_t led_timer = 0;
uint8_t overVolWarning = 0;
uint8_t lowVolWarning = 0;

//循环延迟定时器
uint64_t delay_timer = 0;

//记录启动时候，调解电位器的值，用于跳过boost
uint16_t potentiometerValueOnBoot = 0;
//记录进入校准时间
uint64_t start_calibration_timer = 0;

//功率标定数组
uint8_t powerCalibrationArray[16];

//气压传感器初始化标志
bool bmp_init_flag[2];

#ifdef DEBUG
uint64_t debug_printf_timer = 0;
#endif

typedef enum {
	MODE_OFF = 0,
	MODE_AUTO_BOOTING,
	MODE_CALIBRATION_PRESSURE_MAX,
	MODE_AUTO,
	MODE_MANUAL,
	MODE_AUTO_FAILED,
};

int mode = MODE_AUTO_BOOTING;

//计算速度脉冲中断
void speedPulseCounter() {
	speedPulseCount++;
}

// uint8_t my_u8x8_byte_sw_i2c(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr) {
//   uint8_t *data;

//   switch (msg) {
//     case U8X8_MSG_BYTE_SEND:
//       data = (uint8_t *)arg_ptr;
//       while (arg_int > 0) {
//         SWire.write(*data);
//         data++;
//         arg_int--;
//       }
//       break;
//     case U8X8_MSG_BYTE_INIT:
//       break;
//     case U8X8_MSG_BYTE_SET_DC:
//       break;
//     case U8X8_MSG_BYTE_START_TRANSFER:
//       SWire.beginTransmission(0x3C);
//       break;
//     case U8X8_MSG_BYTE_END_TRANSFER:
//       SWire.endTransmission();
//       break;
//     default:
//       return 0;
//   }
//   return 1;
// }
// uint8_t my_u8x8_gpio_and_delay_arduino(u8x8_t* u8x8, uint8_t msg, uint8_t arg_int, U8X8_UNUSED void* arg_ptr)
// {
// 	return 0;
// }
// U8G2 u8g2;   // All Boards without Reset of the Display
#define VOL_PULL_UP_R (10000)
#define VOL_PULL_DOWN_R (1500)
float getSysVol() {
	float vol = 0;
	float vol_adc = analogRead(VOLTAGE_DETECTION);
	vol = (vol_adc / 4096) * 3.3;
	vol = vol * (VOL_PULL_UP_R + VOL_PULL_DOWN_R) / VOL_PULL_DOWN_R;
	//滤波
	sys_vol = sys_vol*0.9 + vol*0.1;
	return sys_vol;
}

#define CUR_R1 (1010)  //连接到运算放大器的In+和输入+，另外一个端口连接到运算放大器的In-和输入-
#define CUR_R2 (47000)  //连接到运算放大器的In-和输出，用来提高放大倍数
#define CUR_SAMPLING_RESISTOR (0.03) //采样电阻
float getSysCur() {
	float amplifier_output_vol = analogRead(CURRENT_DETECTION)* 3.3 / 4096;
	float cur_sampling_resistor_vol = amplifier_output_vol / (CUR_R2 / CUR_R1);
	//滤波
	sys_cur = sys_cur*0.9 + (cur_sampling_resistor_vol / CUR_SAMPLING_RESISTOR)* 0.1;
	return sys_cur;
}


void setup()
{
	//初始化串口
	#ifdef Serial
		Serial.begin(115200);
	#endif

	//初始化软件I2C
	SWire.begin();
	// Wire.begin();

	//初始化GPIO
	pinMode(CURRENT_DETECTION, INPUT);
	pinMode(POTENTIOMETER_INPUT_PIN, INPUT);
	pinMode(VOLTAGE_DETECTION, INPUT);
	pinMode(BOOT0_PIN, INPUT_PULLDOWN);
	pinMode(DC_CHECK, INPUT_PULLUP);
	pinMode(MODE_SELECT_INPUT_PIN, INPUT_PULLUP);
	pinMode(SPEED_PULSE_INPUT, INPUT_PULLUP);
	pinMode(LED_OUTPUT_PIN, OUTPUT);
	digitalWrite(LED_OUTPUT_PIN, HIGH);

	//ADC分辨率
	analogReadResolution(12);
	//读取电位器数值
	potentiometerValueOnBoot = analogRead(POTENTIOMETER_INPUT_PIN);

	

	//设置PWM输出频率
	analogWriteFrequency(18000L);
	analogWrite(PWM_OUTPUT_PIN, (PWM_MAX - PWM_MIN)/2);
	delay(500);

	//QC协议请求12V，前提是DC_CHECK为低电平
	if (!digitalRead(DC_CHECK)) {
		quickCharge.begin();
		quickCharge.set12V();
	}

	//初始化 速度脉冲 捕获 定时器
	timer.setMode(3, TIMER_INPUT_CAPTURE_FALLING, SPEED_PULSE_INPUT); // 使用通道1捕获PA0的上升沿
	timer.attachInterrupt(3, speedPulseCounter);
	timer.resume();

	//turn the PID on
	myPID.SetMode(AUTOMATIC);
	myPID.SetSampleTime(PID_CAL_TIME);
	myPID.SetOutputLimits(PWM_MIN, PWM_MAX);

	
	//label init_bmp280:
	init_bmp280:
	bmp_init_flag[0] = bmp_env.begin(BMP280_ADDRESS, BMP280_CHIPID);
	bmp_init_flag[1] = bmp_intel.begin(BMP280_ADDRESS_ALT, BMP280_CHIPID);

	if (!bmp_init_flag[0] || !bmp_init_flag[1]) {
		#ifdef DEBUG
			// Serial.println("[失败]BMP280 [0]:%d, [1]:%d\n", bmp_init_flag[0], bmp_init_flag[1]);
			Serial.print("[失败]BMP280 [0]:");
			Serial.print(bmp_init_flag[0]);
			Serial.print(", [1]:");
			Serial.print(bmp_init_flag[1]);
			Serial.println();
			// goto init_bmp280;
		#endif
		//传感器初始化失败，强制进入手动模式
		mode = MODE_AUTO_FAILED;
	}else{
		//设置气压传感器
		bmp_env.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
						Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
						Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
						Adafruit_BMP280::FILTER_X16,      /* Filtering. */
						Adafruit_BMP280::STANDBY_MS_63); /* Standby time. */

		bmp_intel.setSampling(Adafruit_BMP280::MODE_NORMAL,     /* Operating Mode. */
						Adafruit_BMP280::SAMPLING_X2,     /* Temp. oversampling */
						Adafruit_BMP280::SAMPLING_X16,    /* Pressure oversampling */
						Adafruit_BMP280::FILTER_X16,      /* Filtering. */
						Adafruit_BMP280::STANDBY_MS_63); /* Standby time. */
	}
	Serial.println("READY\n");

	
	
	// u8g2_Setup_ssd1305_i2c_128x32_noname_1(&u8g2.u8g2, U8G2_R0, my_u8x8_byte_sw_i2c, my_u8x8_gpio_and_delay_arduino);
	// u8g2.begin();
	// u8g2.firstPage();
	// do {
	// 	// u8g2.setFont(u8g2_font_ncenB10_tr);
	// 	// u8g2.drawStr(0,24,"Hello World!");
  //   u8g2.drawHLine(0, 32, 128);
	// } while ( u8g2.nextPage() );

}

uint64_t qcHandshake_CD_Timer = 0;
#define QC_HANDSHAKE_CD_TIME (1000)

void loop()
{

	// 如果boot0引脚为高电平，则强制进入手动模式
	if (digitalRead(BOOT0_PIN)) {
		#ifdef DEBUG
			Serial.println("boot0 high, force manual mode\n");
		#endif
		mode = MODE_AUTO_FAILED;
	}

	//如果在MODE_AUTO_BOOTING或者MODE_CALIBRATION_PRESSURE_MAX模式下，电位器数值发生改变，并且相比启动时的数值变化操过50，则进入强制模式
	if ((mode == MODE_AUTO_BOOTING || mode == MODE_CALIBRATION_PRESSURE_MAX) && 
		abs((int)(analogRead(POTENTIOMETER_INPUT_PIN) - potentiometerValueOnBoot)) > 256)
	{
		#ifdef DEBUG
			Serial.println("potentiometer changed, force manual mode\n");
		#endif
		mode = MODE_AUTO_FAILED;
	}
	


	if (mode != MODE_AUTO_FAILED) {
		if (millis() - bmp280_sampling_timer > BMP280_SAMPLING_TIME) {
			//读取内部气压
			pressure_inter_last = pressure_inter;
			pressure_inter = bmp_intel.readPressure();
			//读取环境气压
			pressure_env = bmp_env.readPressure();

			bmp280_sampling_timer = millis();
		}
	}

	//电位器采样
	//注意constrain是個宏定義，所以analogRead可能會被調用兩次，導致約束失敗
	// potentiometerValue = constrain(analogRead(POTENTIOMETER_INPUT_PIN), POTENTIOMETER_INPUT_MIN, POTENTIOMETER_INPUT_MAX);
	potentiometerValue = analogRead(POTENTIOMETER_INPUT_PIN);
	potentiometerValue = constrain(potentiometerValue, POTENTIOMETER_INPUT_MIN, POTENTIOMETER_INPUT_MAX);
	// if (potentiometerValue<POTENTIOMETER_INPUT_MIN) potentiometerValue = POTENTIOMETER_INPUT_MIN;
	// else if (potentiometerValue>POTENTIOMETER_INPUT_MAX) potentiometerValue = POTENTIOMETER_INPUT_MAX;

	//电压采样
	getSysVol();
	//电流采样
	getSysCur();

	if (sys_vol > 14) {
		overVolWarning = 1;
		//前提是处于typec模式
		if (MANDATORY_USE_OF_QC_PROTOCOL || !digitalRead(DC_CHECK)) {
			#ifdef DEBUG
				Serial.println("过压！请求QC协议 5V\n");
			#endif
			quickCharge.begin();
			quickCharge.set5V();
		}
		
	}
	else if (sys_vol < 9 && !overVolWarning) {
		lowVolWarning = 1;
		//前提是处于typec模式
		if (millis() - qcHandshake_CD_Timer >= QC_HANDSHAKE_CD_TIME && MANDATORY_USE_OF_QC_PROTOCOL || !digitalRead(DC_CHECK)) {
			#ifdef DEBUG
				Serial.println("请求QC协议 12V\n");
			#endif
			quickCharge.begin();
			quickCharge.set12V();

			qcHandshake_CD_Timer = millis();
		}
	}else lowVolWarning = 0;

	//系统状态机
	switch (mode) {
		case MODE_OFF:{
			pwm = PWM_MIN;
		}break;
		case MODE_AUTO_BOOTING: {
			pwm = ((PWM_MAX - PWM_MIN)/4)*3;
			mode = MODE_CALIBRATION_PRESSURE_MAX;
		}break;
		case MODE_CALIBRATION_PRESSURE_MAX: {
			//设置最大输出
			pwm = PWM_MAX;
			if (abs(pressure_inter - pressure_inter_last) < PRESSURE_STABLE_DELTA && pressure_inter > pressure_env + 200) {
				//最大输出稳定
				pressure_inter_max_delta = pressure_inter - pressure_env;
				mode = MODE_AUTO;
			}
			//校准超时，则强制进入手动模式
			if (millis() > start_calibration_timer + 15 * 1000) mode = MODE_AUTO_FAILED;
		}break;
		case MODE_AUTO: {
			if (millis() - mode_auto_timer > PID_CAL_TIME) {
				Input = pressure_inter/10 - map(potentiometerValue,
												POTENTIOMETER_INPUT_MIN,
												POTENTIOMETER_INPUT_MAX,
												pressure_env,
												pressure_env + pressure_inter_max_delta)/10;
				myPID.Compute();
				pwm = Output;
				//重置PID计算计数器
				mode_auto_timer = millis();
			}
		}break;
		case MODE_AUTO_FAILED:
		case MODE_MANUAL: {
			//注意不能不输出pwm，这会导致电机主控关机
			//故障安全:倒过来map，防止ADC启动失败后陷入危险区，Air001的芯片有坑
			pwm = map(potentiometerValue,
					  POTENTIOMETER_INPUT_MIN,
					  POTENTIOMETER_INPUT_MAX,
					  PWM_MIN, PWM_MAX);
		}break;
	}
    //防止过流
	if (pwm < pwm_real) pwm_real = pwm;
	if (pwm_last_adjust_timer + pwm_adjust_interval < millis()) {
		if (pwm_real < pwm) {
			instantaneous_power_detla = ((sys_vol * sys_cur) - instantaneous_power_last)/((millis()-pwm_last_adjust_timer)/1000.0);
			instantaneous_power_last = (sys_vol * sys_cur);

			float instantaneous_power_limit = 12;
			float instantaneous_power_limit_delta = instantaneous_power_limit - instantaneous_power_detla;

			if (instantaneous_power_limit_delta < 0) pwm_adjust_interval = 2;
			else if (instantaneous_power_limit_delta > instantaneous_power_limit) pwm_adjust_interval = 30;
			else pwm_adjust_interval = instantaneous_power_limit_delta * (30 - 2) / instantaneous_power_limit + 2;

			if (speedPulse<8500) pwm_real += constrain(4, 0, pwm - pwm_real);
			else pwm_real += constrain(6, 0, pwm - pwm_real);
			
			pwm_last_adjust_timer = millis();
		}
	}
	// float vol_limit_ratio = constrain((sys_vol/11.4), 0, 1);
	// float cur_limit_ratio = constrain((1.45/sys_cur), 0, 1);
	// //如果电压低于11V，并且电流大于1.4A 则需要采取措施：降低PWM输出，这个减少是线性的，如果电压低于12V，或者电流高于1.4A，偏差越大，限制越大
	// if (vol_limit_ratio >= 0.99 && cur_limit_ratio >= 0.99) pwm_real += (pwm - pwm_real) * 0.2;
	// else if (speedPulse<8500 && sys_cur>1200) pwm_real = pwm_real * min(vol_limit_ratio * 0.4, cur_limit_ratio * 0.7);
	// if (pwm - pwm_real < 3) pwm_real = pwm;


	//设置输出
	if (lastPwm != pwm) {
		// pwm_real = pwm;
		analogWrite(PWM_OUTPUT_PIN, pwm_real);
		lastPwm = pwm_real;
	}

	//速度脉冲计数器
	if (millis() - speedPulseTimer > 100) {
		speedPulse = speedPulseCount * 10 * SPEED_RATIO;
		speedPulseCount = 0;
		speedPulseTimer = millis();
	}

	#ifdef DEBUG
	//调试输出
	if (millis() - debug_printf_timer > 10 || 1) {
		// printf("mode:%d pwm:%d pot:%d sp:%d p:%d pe:%d pim:%d\n",
		// 		mode, pwm, potentiometerValue, speedPulse, (int)pressure_inter, (int)pressure_env, (int)pressure_inter_max);
		//copilot 帮我改写为 Serial.print的形式
		Serial.print("mode:");
		Serial.print(mode);
		Serial.print(" vol(mv):");
		Serial.print((int)(sys_vol*1000));
		Serial.print(" cor(ma):");
		Serial.print((int)(sys_cur*1000));
		Serial.print(" instantaneous_power_detla:");
		Serial.print((int)(instantaneous_power_detla));
		Serial.print(" pwm_adjust_interval:");
		Serial.print(pwm_adjust_interval);
		Serial.print(" pwm:");
		Serial.print(pwm);
		Serial.print(" pwm_real:");
		Serial.print(pwm_real);
		Serial.print(" pot:");
		Serial.print(potentiometerValue);
		Serial.print(" sp:");
		Serial.print(speedPulse);
		Serial.print(" p:");
		Serial.print((int)pressure_inter);
		Serial.print(" pe:");
		Serial.print((int)pressure_env);
		Serial.print(" pimd:");
		Serial.print((int)pressure_inter_max_delta);
		Serial.println();

		
		debug_printf_timer = millis();
	}
	#endif

	static uint16_t blinkPeriod = 1000;

	if (lowVolWarning) {
	blinkPeriod = 500;  // lowVolWarning情况下500ms一次周期
	} else if (!overVolWarning) {
	blinkPeriod = 7500;  // !overVolWarning情况下每秒完成一次周期
	} else {
	blinkPeriod = 100;  // overVolWarning情况下100ms一次周期
	}

	// 计算LED亮度
	int brightness = 0;
	if (blinkPeriod >= 1000) {
		if (millis() % blinkPeriod < blinkPeriod / 4) {
			brightness = map(millis() % blinkPeriod, 0, blinkPeriod / 4, 0, 255);
		}
		else if (millis() % blinkPeriod < blinkPeriod / 2) {
			brightness = map(millis() % blinkPeriod, blinkPeriod / 4, blinkPeriod / 2, 255, 0);
		}
		else brightness = 0;
	}
	else {
		//直接闪烁，没有呼吸效果
		if (millis() % blinkPeriod < blinkPeriod / 2) brightness = 255;
		else brightness = 0;
	}

	// 设置LED亮度
	analogWrite(LED_OUTPUT_PIN, 255 - brightness);

	if (mode == MODE_CALIBRATION_PRESSURE_MAX || mode == MODE_AUTO_BOOTING) {
		delay(250);
	}
	
	if (digitalRead(MODE_SELECT_INPUT_PIN)){
		mode = MODE_MANUAL;
	}else {
		//尝试启动呼吸跟随模式
		//判断一下气压传感器是否初始化正常
		if (bmp_init_flag[0] && bmp_init_flag[1]) {
			//进入呼吸跟随模式前需要重新校准
			if (mode!=MODE_AUTO_BOOTING && mode!=MODE_CALIBRATION_PRESSURE_MAX && mode!=MODE_AUTO && mode!=MODE_AUTO_FAILED) {
				mode = MODE_AUTO_BOOTING;
				potentiometerValueOnBoot = analogRead(POTENTIOMETER_INPUT_PIN);
				start_calibration_timer = millis();
			}
		}else{
			//气压传感器初始化失败，强制进入手动模式
			mode = MODE_AUTO_FAILED;
		}
	}

}
